/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/


#include "precomp.hpp"
//#include "cvtypes.h"
#include <float.h>
#include <limits.h>
//#include "cv.h"

#include <stdio.h>

void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat** projMatrs, CvMat** presPoints, CvMat* points4D, int numImages, CvMat** projError = 0);

/* Valery Mosyagin */

/* If you want to save internal debug info to files uncomment next lines and set paths to files if need */
/* Note these file may be very large */
/*
#define TRACK_BUNDLE
#define TRACK_BUNDLE_FILE            "d:\\test\\bundle.txt"
#define TRACK_BUNDLE_FILE_JAC        "d:\\test\\bundle.txt"
#define TRACK_BUNDLE_FILE_JACERRPROJ "d:\\test\\JacErrProj.txt"
#define TRACK_BUNDLE_FILE_JACERRPNT  "d:\\test\\JacErrPoint.txt"
#define TRACK_BUNDLE_FILE_MATRW      "d:\\test\\matrWt.txt"
#define TRACK_BUNDLE_FILE_DELTAP     "d:\\test\\deltaP.txt"
*/
#define TRACK_BUNDLE_FILE            "d:\\test\\bundle.txt"


/* ============== Bundle adjustment optimization ================= */
void icvComputeDerivateProj(CvMat* points4D, CvMat* projMatr, CvMat* status, CvMat* derivProj) {
	/* Compute derivate for given projection matrix points and status of points */

	CV_FUNCNAME( "icvComputeDerivateProj" );
	__BEGIN__;


	/* ----- Test input params for errors ----- */
	if ( points4D == 0 || projMatr == 0 || status == 0 || derivProj == 0) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	if ( !CV_IS_MAT(points4D) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix 4xN" );
	}

	/* Compute number of points */
	int numPoints;
	numPoints = points4D->cols;

	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
	}

	if ( points4D->rows != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of coordinates of points4D must be 4" );
	}

	if ( !CV_IS_MAT(projMatr) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" );
	}

	if ( projMatr->rows != 3 || projMatr->cols != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" );
	}

	if ( !CV_IS_MAT(status) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" );
	}

	if ( status->rows != 1 || status->cols != numPoints ) {
		CV_ERROR( CV_StsOutOfRange, "Size of status of points must be 1xN" );
	}

	if ( !CV_IS_MAT(derivProj) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "derivProj must be a matrix VisN x 12" );
	}

	if ( derivProj->cols != 12 ) {
		CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix VisN x 12" );
	}
	/* ----- End test ----- */

	int i;

	/* Allocate memory for derivates */

	double p[12];
	/* Copy projection matrix */
	for ( i = 0; i < 12; i++ ) {
		p[i] = cvmGet(projMatr, i / 4, i % 4);
	}

	/* Fill deriv matrix */
	int currVisPoint;
	int currPoint;

	currVisPoint = 0;
	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
		if ( cvmGet(status, 0, currPoint) > 0 ) {
			double X[4];
			X[0] = cvmGet(points4D, 0, currVisPoint);
			X[1] = cvmGet(points4D, 1, currVisPoint);
			X[2] = cvmGet(points4D, 2, currVisPoint);
			X[3] = cvmGet(points4D, 3, currVisPoint);

			/* Compute derivate for this point */

			double piX[3];
			piX[0] = X[0] * p[0] + X[1] * p[1] + X[2] * p[2]  + X[3] * p[3];
			piX[1] = X[0] * p[4] + X[1] * p[5] + X[2] * p[6]  + X[3] * p[7];
			piX[2] = X[0] * p[8] + X[1] * p[9] + X[2] * p[10] + X[3] * p[11];

			int i;
			/* fill derivate by point */

			double tmp3 = 1 / (piX[2] * piX[2]);

			double tmp1 = -piX[0] * tmp3;
			double tmp2 = -piX[1] * tmp3;

			/* fill derivate by projection matrix */
			for ( i = 0; i < 4; i++ ) {
				/* derivate for x */
				cvmSet(derivProj, currVisPoint * 2, i, X[i] / piX[2]); //x' p1i
				cvmSet(derivProj, currVisPoint * 2, 4 + i, 0); //x' p1i
				cvmSet(derivProj, currVisPoint * 2, 8 + i, X[i]*tmp1); //x' p3i

				/* derivate for y */
				cvmSet(derivProj, currVisPoint * 2 + 1, i, 0); //y' p2i
				cvmSet(derivProj, currVisPoint * 2 + 1, 4 + i, X[i] / piX[2]); //y' p2i
				cvmSet(derivProj, currVisPoint * 2 + 1, 8 + i, X[i]*tmp2); //y' p3i
			}

			currVisPoint++;
		}
	}

	if ( derivProj->rows != currVisPoint * 2 ) {
		CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix 2VisN x 12" );
	}


	__END__;
	return;
}
/*======================================================================================*/

void icvComputeDerivateProjAll(CvMat* points4D, CvMat** projMatrs, CvMat** pointPres, int numImages, CvMat** projDerives) {
	CV_FUNCNAME( "icvComputeDerivateProjAll" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}
	if ( projMatrs == 0 || pointPres == 0 || projDerives == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}
	/* ----- End test ----- */

	int currImage;
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		icvComputeDerivateProj(points4D, projMatrs[currImage], pointPres[currImage], projDerives[currImage]);
	}

	__END__;
	return;
}
/*======================================================================================*/

void icvComputeDerivatePoints(CvMat* points4D, CvMat* projMatr, CvMat* presPoints, CvMat* derivPoint) {

	CV_FUNCNAME( "icvComputeDerivatePoints" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( points4D == 0 || projMatr == 0 || presPoints == 0 || derivPoint == 0) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	if ( !CV_IS_MAT(points4D) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix N x 4" );
	}

	int numPoints;
	numPoints = presPoints->cols;

	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );
	}

	if ( points4D->rows != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "points4D must be a matrix N x 4" );
	}

	if ( !CV_IS_MAT(projMatr) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" );
	}

	if ( projMatr->rows != 3 || projMatr->cols != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" );
	}

	if ( !CV_IS_MAT(presPoints) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" );
	}

	if ( presPoints->rows != 1 || presPoints->cols != numPoints ) {
		CV_ERROR( CV_StsOutOfRange, "Size of presPoints status must be 1xN" );
	}

	if ( !CV_IS_MAT(derivPoint) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" );
	}
	/* ----- End test ----- */

	/* Compute derivates by points */

	double p[12];
	int i;
	for ( i = 0; i < 12; i++ ) {
		p[i] = cvmGet(projMatr, i / 4, i % 4);
	}

	int currVisPoint;
	int currProjPoint;

	currVisPoint = 0;
	for ( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) {
		if ( cvmGet(presPoints, 0, currProjPoint) > 0 ) {
			double X[4];
			X[0] = cvmGet(points4D, 0, currProjPoint);
			X[1] = cvmGet(points4D, 1, currProjPoint);
			X[2] = cvmGet(points4D, 2, currProjPoint);
			X[3] = cvmGet(points4D, 3, currProjPoint);

			double piX[3];
			piX[0] = X[0] * p[0] + X[1] * p[1] + X[2] * p[2]  + X[3] * p[3];
			piX[1] = X[0] * p[4] + X[1] * p[5] + X[2] * p[6]  + X[3] * p[7];
			piX[2] = X[0] * p[8] + X[1] * p[9] + X[2] * p[10] + X[3] * p[11];

			int i, j;

			double tmp3 = 1 / (piX[2] * piX[2]);

			for ( j = 0; j < 2; j++ ) { //for x and y
				for ( i = 0; i < 4; i++ ) { // for X,Y,Z,W
					cvmSet( derivPoint,
							j, currVisPoint * 4 + i,
							(p[j*4+i]*piX[2] - p[8+i]*piX[j]) * tmp3  );
				}
			}
			currVisPoint++;
		}
	}

	if ( derivPoint->rows != 2 || derivPoint->cols != currVisPoint * 4 ) {
		CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" );
	}

	__END__;
	return;
}
/*======================================================================================*/
void icvComputeDerivatePointsAll(CvMat* points4D, CvMat** projMatrs, CvMat** pointPres, int numImages, CvMat** pointDerives) {
	CV_FUNCNAME( "icvComputeDerivatePointsAll" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}
	if ( projMatrs == 0 || pointPres == 0 || pointDerives == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}
	/* ----- End test ----- */

	int currImage;
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		icvComputeDerivatePoints(points4D, projMatrs[currImage], pointPres[currImage], pointDerives[currImage]);
	}

	__END__;
	return;
}
/*======================================================================================*/
void icvComputeMatrixVAll(int numImages, CvMat** pointDeriv, CvMat** presPoints, CvMat** matrV) {
	int* shifts = 0;

	CV_FUNCNAME( "icvComputeMatrixVAll" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}
	if ( pointDeriv == 0 || presPoints == 0 || matrV == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}
	/*  !!! not tested all parameters */
	/* ----- End test ----- */

	/* Compute all matrices U */
	int currImage;
	int currPoint;
	int numPoints;
	numPoints = presPoints[0]->cols;
	CV_CALL(shifts = (int*)cvAlloc(sizeof(int) * numImages));
	memset(shifts, 0, sizeof(int)*numImages);

	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) { //For each point (matrix V)
		int i, j;

		for ( i = 0; i < 4; i++ ) {
			for ( j = 0; j < 4; j++ ) {
				double sum = 0;
				for ( currImage = 0; currImage < numImages; currImage++ ) {
					if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {
						sum += cvmGet(pointDeriv[currImage], 0, shifts[currImage] * 4 + i) *
							   cvmGet(pointDeriv[currImage], 0, shifts[currImage] * 4 + j);

						sum += cvmGet(pointDeriv[currImage], 1, shifts[currImage] * 4 + i) *
							   cvmGet(pointDeriv[currImage], 1, shifts[currImage] * 4 + j);
					}
				}

				cvmSet(matrV[currPoint], i, j, sum);
			}
		}


		/* shift position of visible points */
		for ( currImage = 0; currImage < numImages; currImage++ ) {
			if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {
				shifts[currImage]++;
			}
		}
	}

	__END__;
	cvFree( &shifts);

	return;
}
/*======================================================================================*/
void icvComputeMatrixUAll(int numImages, CvMat** projDeriv, CvMat** matrU) {
	CV_FUNCNAME( "icvComputeMatrixVAll" );
	__BEGIN__;
	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}
	if ( projDeriv == 0 || matrU == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}
	/* !!! Not tested all input parameters */
	/* ----- End test ----- */

	/* Compute matrices V */
	int currImage;
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		cvMulTransposed(projDeriv[currImage], matrU[currImage], 1);
	}

	__END__;
	return;
}
/*======================================================================================*/
void icvComputeMatrixW(int numImages, CvMat** projDeriv, CvMat** pointDeriv, CvMat** presPoints, CvMat* matrW) {
	CV_FUNCNAME( "icvComputeMatrixW" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}
	if ( projDeriv == 0 || pointDeriv == 0 || presPoints == 0 || matrW == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}
	int numPoints;
	numPoints = presPoints[0]->cols;
	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" );
	}
	if ( !CV_IS_MAT(matrW) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "matrW must be a matrix 12NumIm x 4NumPnt" );
	}
	if ( matrW->rows != numImages * 12 || matrW->cols != numPoints * 4 ) {
		CV_ERROR( CV_StsOutOfRange, "matrW must be a matrix 12NumIm x 4NumPnt" );
	}
	/* !!! Not tested all input parameters */
	/* ----- End test ----- */

	/* Compute number of points */
	/* Compute matrix W using derivate proj and points */

	int currImage;

	for ( currImage = 0; currImage < numImages; currImage++ ) {
		for ( int currLine = 0; currLine < 12; currLine++ ) {
			int currVis = 0;
			for ( int currPoint = 0; currPoint < numPoints; currPoint++ ) {
				if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {

					for ( int currCol = 0; currCol < 4; currCol++ ) {
						double sum;
						sum = cvmGet(projDeriv[currImage], currVis * 2 + 0, currLine) *
							  cvmGet(pointDeriv[currImage], 0, currVis * 4 + currCol);

						sum += cvmGet(projDeriv[currImage], currVis * 2 + 1, currLine) *
							   cvmGet(pointDeriv[currImage], 1, currVis * 4 + currCol);

						cvmSet(matrW, currImage * 12 + currLine, currPoint * 4 + currCol, sum);
					}
					currVis++;
				} else {
					/* set all sub elements to zero */
					for ( int currCol = 0; currCol < 4; currCol++ ) {
						cvmSet(matrW, currImage * 12 + currLine, currPoint * 4 + currCol, 0);
					}
				}
			}
		}
	}

#ifdef TRACK_BUNDLE
	{
		FILE* file;
		file = fopen( TRACK_BUNDLE_FILE_MATRW , "w");
		int currPoint, currImage;
		for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
			fprintf(file, "\nPoint=%d\n", currPoint);
			int currRow;
			for ( currRow = 0; currRow < 4; currRow++  ) {
				for ( currImage = 0; currImage < numImages; currImage++ ) {
					int i;
					for ( i = 0; i < 12; i++ ) {
						double val = cvmGet(matrW, currImage * 12 + i, currPoint * 4 + currRow);
						fprintf(file, "%lf ", val);
					}
				}
				fprintf(file, "\n");
			}
		}
		fclose(file);
	}
#endif

	__END__;
	return;
}
/*======================================================================================*/
/* Compute jacobian mult projection matrices error */
void icvComputeJacErrorProj(int numImages, CvMat** projDeriv, CvMat** projErrors, CvMat* jacProjErr ) {
	CV_FUNCNAME( "icvComputeJacErrorProj" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}
	if ( projDeriv == 0 || projErrors == 0 || jacProjErr == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}
	if ( !CV_IS_MAT(jacProjErr) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "jacProjErr must be a matrix 12NumIm x 1" );
	}
	if ( jacProjErr->rows != numImages * 12 || jacProjErr->cols != 1 ) {
		CV_ERROR( CV_StsOutOfRange, "jacProjErr must be a matrix 12NumIm x 1" );
	}
	/* !!! Not tested all input parameters */
	/* ----- End test ----- */

	int currImage;
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		for ( int currCol = 0; currCol < 12; currCol++ ) {
			int num = projDeriv[currImage]->rows;
			double sum = 0;
			for ( int i = 0; i < num; i++ ) {
				sum += cvmGet(projDeriv[currImage], i, currCol) *
					   cvmGet(projErrors[currImage], i % 2, i / 2);
			}
			cvmSet(jacProjErr, currImage * 12 + currCol, 0, sum);
		}
	}

#ifdef TRACK_BUNDLE
	{
		FILE* file;
		file = fopen( TRACK_BUNDLE_FILE_JACERRPROJ , "w");
		int currImage;
		for ( currImage = 0; currImage < numImages; currImage++ ) {
			fprintf(file, "\nImage=%d\n", currImage);
			int currRow;
			for ( currRow = 0; currRow < 12; currRow++  ) {
				double val = cvmGet(jacProjErr, currImage * 12 + currRow, 0);
				fprintf(file, "%lf\n", val);
			}
			fprintf(file, "\n");
		}
		fclose(file);
	}
#endif


	__END__;
	return;
}
/*======================================================================================*/
/* Compute jacobian mult points error */
void icvComputeJacErrorPoint(int numImages, CvMat** pointDeriv, CvMat** projErrors, CvMat** presPoints, CvMat* jacPointErr ) {
	int* shifts = 0;

	CV_FUNCNAME( "icvComputeJacErrorPoint" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
	}

	if ( pointDeriv == 0 || projErrors == 0 || presPoints == 0 || jacPointErr == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	int numPoints;
	numPoints = presPoints[0]->cols;
	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" );
	}

	if ( !CV_IS_MAT(jacPointErr) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "jacPointErr must be a matrix 4NumPnt x 1" );
	}

	if ( jacPointErr->rows != numPoints * 4 || jacPointErr->cols != 1 ) {
		CV_ERROR( CV_StsOutOfRange, "jacPointErr must be a matrix 4NumPnt x 1" );
	}
	/* !!! Not tested all input parameters */
	/* ----- End test ----- */

	int currImage;
	int currPoint;
	CV_CALL(shifts = (int*)cvAlloc(sizeof(int) * numImages));
	memset(shifts, 0, sizeof(int)*numImages);
	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
		for ( int currCoord = 0; currCoord < 4; currCoord++ ) {
			double sum = 0;
			{
				int currVis = 0;
				for ( currImage = 0; currImage < numImages; currImage++ ) {
					if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {
						sum += cvmGet(pointDeriv[currImage], 0, shifts[currImage] * 4 + currCoord) *
							   cvmGet(projErrors[currImage], 0, shifts[currImage]); //currVis);

						sum += cvmGet(pointDeriv[currImage], 1, shifts[currImage] * 4 + currCoord) *
							   cvmGet(projErrors[currImage], 1, shifts[currImage]); //currVis);

						currVis++;
					}
				}
			}

			cvmSet(jacPointErr, currPoint * 4 + currCoord, 0, sum);
		}

		/* Increase shifts */
		for ( currImage = 0; currImage < numImages; currImage++ ) {
			if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {
				shifts[currImage]++;
			}
		}
	}


#ifdef TRACK_BUNDLE
	{
		FILE* file;
		file = fopen(TRACK_BUNDLE_FILE_JACERRPNT, "w");
		int currPoint;
		for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
			fprintf(file, "\nPoint=%d\n", currPoint);
			int currRow;
			for ( currRow = 0; currRow < 4; currRow++  ) {
				double val = cvmGet(jacPointErr, currPoint * 4 + currRow, 0);
				fprintf(file, "%lf\n", val);
			}
			fprintf(file, "\n");
		}
		fclose(file);
	}
#endif



	__END__;
	cvFree( &shifts);

}
/*======================================================================================*/

/* Reconstruct 4D points using status */
void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat** projMatrs, CvMat** presPoints,
								  CvMat* points4D, int numImages, CvMat** projError) {

	double* matrA_dat = 0;
	double* matrW_dat = 0;

	CV_FUNCNAME( "icvReconstructPoints4DStatus" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 2 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must be more than one" );
	}

	if ( projPoints == 0 || projMatrs == 0 || presPoints == 0 || points4D == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	int numPoints;
	numPoints = points4D->cols;
	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
	}

	if ( points4D->rows != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" );
	}

	/* !!! Not tested all input parameters */
	/* ----- End test ----- */

	int currImage;
	int currPoint;

	/* Allocate maximum data */


	CvMat matrV;
	double matrV_dat[4*4];
	matrV = cvMat(4, 4, CV_64F, matrV_dat);

	CV_CALL(matrA_dat = (double*)cvAlloc(3 * numImages * 4 * sizeof(double)));
	CV_CALL(matrW_dat = (double*)cvAlloc(3 * numImages * 4 * sizeof(double)));

	/* reconstruct each point */
	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
		/* Reconstruct current point */
		/* Define number of visible projections */
		int numVisProj = 0;
		for ( currImage = 0; currImage < numImages; currImage++ ) {
			if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {
				numVisProj++;
			}
		}

		if ( numVisProj < 2 ) {
			/* This point can't be reconstructed */
			continue;
		}

		/* Allocate memory and create matrices */
		CvMat matrA;
		matrA = cvMat(3 * numVisProj, 4, CV_64F, matrA_dat);

		CvMat matrW;
		matrW = cvMat(3 * numVisProj, 4, CV_64F, matrW_dat);

		int currVisProj = 0;
		for ( currImage = 0; currImage < numImages; currImage++ ) { /* For each view */
			if ( cvmGet(presPoints[currImage], 0, currPoint) > 0 ) {
				double x, y;
				x = cvmGet(projPoints[currImage], 0, currPoint);
				y = cvmGet(projPoints[currImage], 1, currPoint);
				for ( int k = 0; k < 4; k++ ) {
					matrA_dat[currVisProj*12   + k] =
						x * cvmGet(projMatrs[currImage], 2, k) -     cvmGet(projMatrs[currImage], 0, k);

					matrA_dat[currVisProj*12+4 + k] =
						y * cvmGet(projMatrs[currImage], 2, k) -     cvmGet(projMatrs[currImage], 1, k);

					matrA_dat[currVisProj*12+8 + k] =
						x * cvmGet(projMatrs[currImage], 1, k) - y * cvmGet(projMatrs[currImage], 0, k);
				}
				currVisProj++;
			}
		}

		/* Solve system for current point */
		{
			cvSVD(&matrA, &matrW, 0, &matrV, CV_SVD_V_T);

			/* Copy computed point */
			cvmSet(points4D, 0, currPoint, cvmGet(&matrV, 3, 0)); //X
			cvmSet(points4D, 1, currPoint, cvmGet(&matrV, 3, 1)); //Y
			cvmSet(points4D, 2, currPoint, cvmGet(&matrV, 3, 2)); //Z
			cvmSet(points4D, 3, currPoint, cvmGet(&matrV, 3, 3)); //W
		}

	}

	{
		/* Compute projection error */
		for ( currImage = 0; currImage < numImages; currImage++ ) {
			CvMat point4D;
			CvMat point3D;
			double point3D_dat[3];
			point3D = cvMat(3, 1, CV_64F, point3D_dat);

			int currPoint;
			int numVis = 0;
			double totalError = 0;
			for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
				if ( cvmGet(presPoints[currImage], 0, currPoint) > 0) {
					double  dx, dy;
					cvGetCol(points4D, &point4D, currPoint);
					cvmMul(projMatrs[currImage], &point4D, &point3D);
					double w = point3D_dat[2];
					double x = point3D_dat[0] / w;
					double y = point3D_dat[1] / w;

					dx = cvmGet(projPoints[currImage], 0, currPoint) - x;
					dy = cvmGet(projPoints[currImage], 1, currPoint) - y;
					if ( projError ) {
						cvmSet(projError[currImage], 0, currPoint, dx);
						cvmSet(projError[currImage], 1, currPoint, dy);
					}
					totalError += sqrt(dx * dx + dy * dy);
					numVis++;
				}
			}

			//double meanError = totalError / (double)numVis;

		}

	}

	__END__;

	cvFree( &matrA_dat);
	cvFree( &matrW_dat);

	return;
}

/*======================================================================================*/

void icvProjPointsStatusFunc( int numImages, CvMat* points4D, CvMat** projMatrs, CvMat** pointsPres, CvMat** projPoints) {
	CV_FUNCNAME( "icvProjPointsStatusFunc" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" );
	}

	if ( points4D == 0 || projMatrs == 0 || pointsPres == 0 || projPoints == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	int numPoints;
	numPoints = points4D->cols;
	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
	}

	if ( points4D->rows != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" );
	}

	/* !!! Not tested all input parameters */
	/* ----- End test ----- */

	CvMat point4D;
	CvMat point3D;
	double point4D_dat[4];
	double point3D_dat[3];
	point4D = cvMat(4, 1, CV_64F, point4D_dat);
	point3D = cvMat(3, 1, CV_64F, point3D_dat);

#ifdef TRACK_BUNDLE
	{
		FILE* file;
		file = fopen( TRACK_BUNDLE_FILE , "a");
		fprintf(file, "\n----- test 14.01 icvProjPointsStatusFunc -----\n");
		fclose(file);
	}
#endif

	int currImage;
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		/* Get project matrix */
		/* project visible points using current projection matrix */
		int currVisPoint = 0;
		for ( int currPoint = 0; currPoint < numPoints; currPoint++ ) {
			if ( cvmGet(pointsPres[currImage], 0, currPoint) > 0 ) {
				/* project current point */
				cvGetSubRect(points4D, &point4D, cvRect(currPoint, 0, 1, 4));

#ifdef TRACK_BUNDLE
				{
					FILE* file;
					file = fopen( TRACK_BUNDLE_FILE , "a");
					fprintf(file, "\n----- test 14.02 point4D (%lf, %lf, %lf, %lf) -----\n",
							cvmGet(&point4D, 0, 0),
							cvmGet(&point4D, 1, 0),
							cvmGet(&point4D, 2, 0),
							cvmGet(&point4D, 3, 0));
					fclose(file);
				}
#endif

				cvmMul(projMatrs[currImage], &point4D, &point3D);
				double w = point3D_dat[2];
				cvmSet(projPoints[currImage], 0, currVisPoint, point3D_dat[0] / w);
				cvmSet(projPoints[currImage], 1, currVisPoint, point3D_dat[1] / w);

#ifdef TRACK_BUNDLE
				{
					FILE* file;
					file = fopen( TRACK_BUNDLE_FILE , "a");
					fprintf(file, "\n----- test 14.03 (%lf, %lf, %lf) -> (%lf, %lf)-----\n",
							point3D_dat[0],
							point3D_dat[1],
							point3D_dat[2],
							point3D_dat[0] / w,
							point3D_dat[1] / w
						   );
					fclose(file);
				}
#endif
				currVisPoint++;
			}
		}
	}

	__END__;
}

/*======================================================================================*/
void icvFreeMatrixArray(CvMat** *matrArray, int numMatr) {
	/* Free each matrix */
	int currMatr;

	if ( *matrArray != 0 ) {
		/* Need delete */
		for ( currMatr = 0; currMatr < numMatr; currMatr++ ) {
			cvReleaseMat((*matrArray) + currMatr);
		}
		cvFree( matrArray);
	}
	return;
}

/*======================================================================================*/
void* icvClearAlloc(int size) {
	void* ptr = 0;

	CV_FUNCNAME( "icvClearAlloc" );
	__BEGIN__;

	if ( size > 0 ) {
		CV_CALL(ptr = cvAlloc(size));
		memset(ptr, 0, size);
	}

	__END__;
	return ptr;
}

/*======================================================================================*/
#if 0
void cvOptimizeLevenbergMarquardtBundleWraper( CvMat** projMatrs, CvMat** observProjPoints,
		CvMat** pointsPres, int numImages,
		CvMat** resultProjMatrs, CvMat* resultPoints4D, int maxIter, double epsilon ) {
	/* Delete al sparse points */

	int icvDeleteSparsInPoints(  int numImages,
								 CvMat** points,
								 CvMat** status,
								 CvMat * wasStatus) /* status of previous configuration */

}
#endif
/*======================================================================================*/
/* !!! may be useful to return norm of error */
/* !!! may be does not work correct with not all visible 4D points */
void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints,
		CvMat** pointsPres, int numImages,
		CvMat** resultProjMatrs, CvMat* resultPoints4D, int maxIter, double epsilon ) {

	CvMat*  vectorX_points4D = 0;
	CvMat** vectorX_projMatrs = 0;

	CvMat*  newVectorX_points4D = 0;
	CvMat** newVectorX_projMatrs = 0;

	CvMat*  changeVectorX_points4D = 0;
	CvMat*  changeVectorX_projMatrs = 0;

	CvMat** observVisPoints = 0;
	CvMat** projVisPoints = 0;
	CvMat** errorProjPoints = 0;
	CvMat** DerivProj = 0;
	CvMat** DerivPoint = 0;
	CvMat* matrW = 0;
	CvMat** matrsUk = 0;
	CvMat** workMatrsUk = 0;
	CvMat** matrsVi = 0;
	CvMat* workMatrVi = 0;
	CvMat** workMatrsInvVi = 0;
	CvMat* jacProjErr = 0;
	CvMat* jacPointErr = 0;

	CvMat* matrTmpSys1 = 0;
	CvMat* matrSysDeltaP = 0;
	CvMat* vectTmpSys3 = 0;
	CvMat* vectSysDeltaP = 0;
	CvMat* deltaP = 0;
	CvMat* deltaM = 0;
	CvMat* vectTmpSysM = 0;

	int numPoints = 0;


	CV_FUNCNAME( "cvOptimizeLevenbergMarquardtBundle" );
	__BEGIN__;

	/* ----- Test input params for errors ----- */
	if ( numImages < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" );
	}

	if ( maxIter < 1 || maxIter > 2000 ) {
		CV_ERROR( CV_StsOutOfRange, "Maximum number of iteration must be in [1..1000]" );
	}

	if ( epsilon < 0  ) {
		CV_ERROR( CV_StsOutOfRange, "Epsilon parameter must be >= 0" );
	}

	if ( !CV_IS_MAT(resultPoints4D) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix 4 x NumPnt" );
	}

	numPoints = resultPoints4D->cols;
	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );//!!!
	}

	if ( resultPoints4D->rows != 4 ) {
		CV_ERROR( CV_StsOutOfRange, "resultPoints4D must have 4 cordinates" );
	}

	/* ----- End test ----- */

	/* Optimization using bundle adjustment */
	/* work with non visible points */

	CV_CALL( vectorX_points4D  = cvCreateMat(4, numPoints, CV_64F));
	CV_CALL( vectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages));

	CV_CALL( newVectorX_points4D  = cvCreateMat(4, numPoints, CV_64F));
	CV_CALL( newVectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages));

	CV_CALL( changeVectorX_points4D  = cvCreateMat(4, numPoints, CV_64F));
	CV_CALL( changeVectorX_projMatrs = cvCreateMat(3, 4, CV_64F));

	int currImage;

	/* ----- Test input params ----- */
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		/* Test size of input initial and result projection matrices */
		if ( !CV_IS_MAT(projMatrs[currImage]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "each of initial projMatrs must be a matrix 3 x 4" );
		}
		if ( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 ) {
			CV_ERROR( CV_StsOutOfRange, "each of initial projMatrs must be a matrix 3 x 4" );
		}


		if ( !CV_IS_MAT(observProjPoints[currImage]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "each of observProjPoints must be a matrix 2 x NumPnts" );
		}
		if ( observProjPoints[currImage]->rows != 2 || observProjPoints[currImage]->cols != numPoints ) {
			CV_ERROR( CV_StsOutOfRange, "each of observProjPoints must be a matrix 2 x NumPnts" );
		}

		if ( !CV_IS_MAT(pointsPres[currImage]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "each of pointsPres must be a matrix 1 x NumPnt" );
		}
		if ( pointsPres[currImage]->rows != 1 || pointsPres[currImage]->cols != numPoints ) {
			CV_ERROR( CV_StsOutOfRange, "each of pointsPres must be a matrix 1 x NumPnt" );
		}

		if ( !CV_IS_MAT(resultProjMatrs[currImage]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "each of resultProjMatrs must be a matrix 3 x 4" );
		}
		if ( resultProjMatrs[currImage]->rows != 3 || resultProjMatrs[currImage]->cols != 4 ) {
			CV_ERROR( CV_StsOutOfRange, "each of resultProjMatrs must be a matrix 3 x 4" );
		}

	}
	/* ----- End test ----- */

	/* Copy projection matrices to vectorX0 */
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		CV_CALL( vectorX_projMatrs[currImage] = cvCreateMat(3, 4, CV_64F));
		CV_CALL( newVectorX_projMatrs[currImage] = cvCreateMat(3, 4, CV_64F));
		cvCopy(projMatrs[currImage], vectorX_projMatrs[currImage]);
	}

	/* Reconstruct points4D using projection matrices and status information */
	icvReconstructPoints4DStatus(observProjPoints, projMatrs, pointsPres, vectorX_points4D, numImages);

	/* ----- Allocate memory for work matrices ----- */
	/* Compute number of good points on each images */

	CV_CALL( observVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( projVisPoints   = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( errorProjPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( DerivProj       = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( DerivPoint      = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( matrW           = cvCreateMat(12 * numImages, 4 * numPoints, CV_64F) );
	CV_CALL( matrsUk         = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( workMatrsUk     = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numImages) );
	CV_CALL( matrsVi         = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numPoints) );
	CV_CALL( workMatrVi      = cvCreateMat(4, 4, CV_64F) );
	CV_CALL( workMatrsInvVi  = (CvMat**)icvClearAlloc(sizeof(CvMat*) * numPoints) );

	CV_CALL( jacProjErr      = cvCreateMat(12 * numImages, 1, CV_64F) );
	CV_CALL( jacPointErr     = cvCreateMat(4 * numPoints, 1, CV_64F) );


	int i;
	for ( i = 0; i < numPoints; i++ ) {
		CV_CALL( matrsVi[i]        = cvCreateMat(4, 4, CV_64F) );
		CV_CALL( workMatrsInvVi[i] = cvCreateMat(4, 4, CV_64F) );
	}

	for ( currImage = 0; currImage < numImages; currImage++ ) {
		CV_CALL( matrsUk[currImage]     = cvCreateMat(12, 12, CV_64F) );
		CV_CALL( workMatrsUk[currImage] = cvCreateMat(12, 12, CV_64F) );

		int currVisPoint = 0, currPoint, numVisPoint;
		for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
			if ( cvmGet(pointsPres[currImage], 0, currPoint) > 0 ) {
				currVisPoint++;
			}
		}

		numVisPoint = currVisPoint;

		/* Allocate memory for current image data */
		CV_CALL( observVisPoints[currImage]  = cvCreateMat(2, numVisPoint, CV_64F) );
		CV_CALL( projVisPoints[currImage]    = cvCreateMat(2, numVisPoint, CV_64F) );

		/* create error matrix */
		CV_CALL( errorProjPoints[currImage] = cvCreateMat(2, numVisPoint, CV_64F) );

		/* Create derivate matrices */
		CV_CALL( DerivProj[currImage]  = cvCreateMat(2 * numVisPoint, 12, CV_64F) );
		CV_CALL( DerivPoint[currImage] = cvCreateMat(2, numVisPoint * 4, CV_64F) );

		/* Copy observed projected visible points */
		currVisPoint = 0;
		for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
			if ( cvmGet(pointsPres[currImage], 0, currPoint) > 0 ) {
				cvmSet(observVisPoints[currImage], 0, currVisPoint, cvmGet(observProjPoints[currImage], 0, currPoint));
				cvmSet(observVisPoints[currImage], 1, currVisPoint, cvmGet(observProjPoints[currImage], 1, currPoint));
				currVisPoint++;
			}
		}
	}
	/*---------------------------------------------------*/

	CV_CALL( matrTmpSys1   = cvCreateMat(numPoints * 4, numImages * 12, CV_64F) );
	CV_CALL( matrSysDeltaP = cvCreateMat(numImages * 12, numImages * 12, CV_64F) );
	CV_CALL( vectTmpSys3   = cvCreateMat(numPoints * 4, 1, CV_64F) );
	CV_CALL( vectSysDeltaP = cvCreateMat(numImages * 12, 1, CV_64F) );
	CV_CALL( deltaP        = cvCreateMat(numImages * 12, 1, CV_64F) );
	CV_CALL( deltaM        = cvCreateMat(numPoints * 4, 1, CV_64F) );
	CV_CALL( vectTmpSysM   = cvCreateMat(numPoints * 4, 1, CV_64F) );


//#ifdef TRACK_BUNDLE
#if 1
	{
		/* Create file to track */
		FILE* file;
		file = fopen( TRACK_BUNDLE_FILE , "w");
		fprintf(file, "begin\n");
		fclose(file);
	}
#endif

	/* ============= main optimization loop ============== */

	/* project all points using current vector X */
	icvProjPointsStatusFunc(numImages, vectorX_points4D, vectorX_projMatrs, pointsPres, projVisPoints);

	/* Compute error with observed value and computed projection */
	double prevError;
	prevError = 0;
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		cvSub(observVisPoints[currImage], projVisPoints[currImage], errorProjPoints[currImage]);
		double currNorm = cvNorm(errorProjPoints[currImage]);
		prevError += currNorm * currNorm;
	}
	prevError = sqrt(prevError);

	int currIter;
	double change;
	double alpha;

//#ifdef TRACK_BUNDLE
#if 1
	{
		/* Create file to track */
		FILE* file;
		file = fopen( TRACK_BUNDLE_FILE , "a");
		fprintf(file, "\n========================================\n");;
		fprintf(file, "Iter=0\n");
		fprintf(file, "Error = %20.15lf\n", prevError);
		fprintf(file, "Change = %20.15lf\n", 1.0);

		fprintf(file, "projection errors\n");

		/* Print all proejction errors */
		int currImage;
		for ( currImage = 0; currImage < numImages; currImage++) {
			fprintf(file, "\nImage=%d\n", currImage);
			int numPn = errorProjPoints[currImage]->cols;
			for ( int currPoint = 0; currPoint < numPn; currPoint++ ) {
				double ex, ey;
				ex = cvmGet(errorProjPoints[currImage], 0, currPoint);
				ey = cvmGet(errorProjPoints[currImage], 1, currPoint);
				fprintf(file, "%40.35lf, %40.35lf\n", ex, ey);
			}
		}
		fclose(file);
	}
#endif

	currIter = 0;
	change = 1;
	alpha = 0.001;


	do {

#ifdef TRACK_BUNDLE
		{
			FILE* file;
			file = fopen( TRACK_BUNDLE_FILE , "a");
			fprintf(file, "\n----- test 6 do main -----\n");

			double norm = cvNorm(vectorX_points4D);
			fprintf(file, "        test 6.01 prev normPnts=%lf\n", norm);

			for ( int i = 0; i < numImages; i++ ) {
				double norm = cvNorm(vectorX_projMatrs[i]);
				fprintf(file, "        test 6.01 prev normProj=%lf\n", norm);
			}

			fclose(file);
		}
#endif

		/* Compute derivates by projectinon matrices */
		icvComputeDerivateProjAll(vectorX_points4D, vectorX_projMatrs, pointsPres, numImages, DerivProj);

		/* Compute derivates by 4D points */
		icvComputeDerivatePointsAll(vectorX_points4D, vectorX_projMatrs, pointsPres, numImages, DerivPoint);

		/* Compute matrces Uk */
		icvComputeMatrixUAll(numImages, DerivProj, matrsUk);
		icvComputeMatrixVAll(numImages, DerivPoint, pointsPres, matrsVi);
		icvComputeMatrixW(numImages, DerivProj, DerivPoint, pointsPres, matrW);


#ifdef TRACK_BUNDLE
		{
			FILE* file;
			file = fopen( TRACK_BUNDLE_FILE , "a");
			fprintf(file, "\n----- test 6.03 do matrs U V -----\n");

			int i;
			for ( i = 0; i < numImages; i++ ) {
				double norm = cvNorm(matrsUk[i]);
				fprintf(file, "        test 6.01 prev matrsUk=%lf\n", norm);
			}

			for ( i = 0; i < numPoints; i++ ) {
				double norm = cvNorm(matrsVi[i]);
				fprintf(file, "        test 6.01 prev matrsVi=%lf\n", norm);
			}

			fclose(file);
		}
#endif

		/* Compute jac errors */
		icvComputeJacErrorProj(numImages, DerivProj, errorProjPoints, jacProjErr);
		icvComputeJacErrorPoint(numImages, DerivPoint, errorProjPoints, pointsPres, jacPointErr);

#ifdef TRACK_BUNDLE
		{
			FILE* file;
			file = fopen( TRACK_BUNDLE_FILE , "a");
			fprintf(file, "\n----- test 6 do main -----\n");
			double norm1 = cvNorm(vectorX_points4D);
			fprintf(file, "        test 6.02 post normPnts=%lf\n", norm1);
			fclose(file);
		}
#endif
		/* Copy matrices Uk to work matrices Uk */
		for ( currImage = 0; currImage < numImages; currImage++ ) {
			cvCopy(matrsUk[currImage], workMatrsUk[currImage]);
		}

#ifdef TRACK_BUNDLE
		{
			FILE* file;
			file = fopen( TRACK_BUNDLE_FILE , "a");
			fprintf(file, "\n----- test 60.3 do matrs U V -----\n");

			int i;
			for ( i = 0; i < numImages; i++ ) {
				double norm = cvNorm(matrsUk[i]);
				fprintf(file, "        test 6.01 post1 matrsUk=%lf\n", norm);
			}

			for ( i = 0; i < numPoints; i++ ) {
				double norm = cvNorm(matrsVi[i]);
				fprintf(file, "        test 6.01 post1 matrsVi=%lf\n", norm);
			}

			fclose(file);
		}
#endif

		/* ========== Solve normal equation for given alpha and Jacobian ============ */

		do {
			/* ---- Add alpha to matrices --- */
			/* Add alpha to matrInvVi and make workMatrsInvVi */

			int currV;
			for ( currV = 0; currV < numPoints; currV++ ) {
				cvCopy(matrsVi[currV], workMatrVi);

				for ( int i = 0; i < 4; i++ ) {
					cvmSet(workMatrVi, i, i, cvmGet(matrsVi[currV], i, i)*(1 + alpha) );
				}

				cvInvert(workMatrVi, workMatrsInvVi[currV], CV_LU/*,&currV*/);
			}

			/* Add alpha to matrUk and make matrix workMatrsUk */
			for ( currImage = 0; currImage < numImages; currImage++ ) {

				for ( i = 0; i < 12; i++ ) {
					cvmSet(workMatrsUk[currImage], i, i, cvmGet(matrsUk[currImage], i, i)*(1 + alpha));
				}


			}

			/* Fill matrix to make system for computing delta P (matrTmpSys1 = inv(V)*tr(W) )*/
			for ( currV = 0; currV < numPoints; currV++ ) {
				int currRowV;
				for ( currRowV = 0; currRowV < 4; currRowV++ ) {
					for ( currImage = 0; currImage < numImages; currImage++ ) {
						for ( int currCol = 0; currCol < 12; currCol++ ) { /* For each column of transposed matrix W */
							double sum = 0;
							for ( i = 0; i < 4; i++ ) {
								sum += cvmGet(workMatrsInvVi[currV], currRowV, i) *
									   cvmGet(matrW, currImage * 12 + currCol, currV * 4 + i);
							}
							cvmSet(matrTmpSys1, currV * 4 + currRowV, currImage * 12 + currCol, sum);
						}
					}
				}
			}


			/* Fill matrix to make system for computing delta P (matrTmpSys2 = W * matrTmpSys ) */
			cvmMul(matrW, matrTmpSys1, matrSysDeltaP);

			/* need to compute U-matrTmpSys2. But we compute matTmpSys2-U */
			for ( currImage = 0; currImage < numImages; currImage++ ) {
				CvMat subMatr;
				cvGetSubRect(matrSysDeltaP, &subMatr, cvRect(currImage * 12, currImage * 12, 12, 12));
				cvSub(&subMatr, workMatrsUk[currImage], &subMatr);
			}

			/* Compute right side of normal equation  */
			for ( currV = 0; currV < numPoints; currV++ ) {
				CvMat subMatrErPnts;
				CvMat subMatr;
				cvGetSubRect(jacPointErr, &subMatrErPnts, cvRect(0, currV * 4, 1, 4));
				cvGetSubRect(vectTmpSys3, &subMatr, cvRect(0, currV * 4, 1, 4));
				cvmMul(workMatrsInvVi[currV], &subMatrErPnts, &subMatr);
			}

			cvmMul(matrW, vectTmpSys3, vectSysDeltaP);
			cvSub(vectSysDeltaP, jacProjErr, vectSysDeltaP);

			/* Now we can compute part of normal system - deltaP */
			cvSolve(matrSysDeltaP , vectSysDeltaP, deltaP, CV_SVD);

			/* Print deltaP to file */

#ifdef TRACK_BUNDLE
			{
				FILE* file;
				file = fopen( TRACK_BUNDLE_FILE_DELTAP , "w");

				int currImage;
				for ( currImage = 0; currImage < numImages; currImage++ ) {
					fprintf(file, "\nImage=%d\n", currImage);
					int i;
					for ( i = 0; i < 12; i++ ) {
						double val;
						val = cvmGet(deltaP, currImage * 12 + i, 0);
						fprintf(file, "%lf\n", val);
					}
					fprintf(file, "\n");
				}
				fclose(file);
			}
#endif
			/* We know deltaP and now we can compute system for deltaM */
			for ( i = 0; i < numPoints * 4; i++ ) {
				double sum = 0;
				for ( int j = 0; j < numImages * 12; j++ ) {
					sum += cvmGet(matrW, j, i) * cvmGet(deltaP, j, 0);
				}
				cvmSet(vectTmpSysM, i, 0, cvmGet(jacPointErr, i, 0) - sum);
			}

			/* Compute deltaM */
			for ( currV = 0; currV < numPoints; currV++ ) {
				CvMat subMatr;
				CvMat subMatrM;
				cvGetSubRect(vectTmpSysM, &subMatr, cvRect(0, currV * 4, 1, 4));
				cvGetSubRect(deltaM, &subMatrM, cvRect(0, currV * 4, 1, 4));
				cvmMul(workMatrsInvVi[currV], &subMatr, &subMatrM);
			}

			/* We know delta and compute new value of vector X: nextVectX = vectX + deltas */

			/* Compute new P */
			for ( currImage = 0; currImage < numImages; currImage++ ) {
				for ( i = 0; i < 3; i++ ) {
					for ( int j = 0; j < 4; j++ ) {
						cvmSet(newVectorX_projMatrs[currImage], i, j,
							   cvmGet(vectorX_projMatrs[currImage], i, j) + cvmGet(deltaP, currImage * 12 + i * 4 + j, 0));
					}
				}
			}

			/* Compute new M */
			int currPoint;
			for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
				for ( i = 0; i < 4; i++ ) {
					cvmSet(newVectorX_points4D, i, currPoint,
						   cvmGet(vectorX_points4D, i, currPoint) + cvmGet(deltaM, currPoint * 4 + i, 0));
				}
			}

			/* ----- Compute errors for new vectorX ----- */
			/* Project points using new vectorX and status of each point */
			icvProjPointsStatusFunc(numImages, newVectorX_points4D, newVectorX_projMatrs, pointsPres, projVisPoints);
			/* Compute error with observed value and computed projection */
			double newError = 0;
			for ( currImage = 0; currImage < numImages; currImage++ ) {
				cvSub(observVisPoints[currImage], projVisPoints[currImage], errorProjPoints[currImage]);
				double currNorm = cvNorm(errorProjPoints[currImage]);

//#ifdef TRACK_BUNDLE
#if 1
				{
					FILE* file;
					file = fopen( TRACK_BUNDLE_FILE , "a");
					fprintf(file, "\n----- test 13,01 currImage=%d currNorm=%lf -----\n", currImage, currNorm);
					fclose(file);
				}
#endif
				newError += currNorm * currNorm;
			}
			newError = sqrt(newError);

			currIter++;




//#ifdef TRACK_BUNDLE
#if 1
			{
				/* Create file to track */
				FILE* file;
				file = fopen( TRACK_BUNDLE_FILE , "a");
				fprintf(file, "\n========================================\n");
				fprintf(file, "numPoints=%d\n", numPoints);
				fprintf(file, "Iter=%d\n", currIter);
				fprintf(file, "Error = %20.15lf\n", newError);
				fprintf(file, "Change = %20.15lf\n", change);


				/* Print all projection errors */
#if 0
				fprintf(file, "projection errors\n");
				int currImage;
				for ( currImage = 0; currImage < numImages; currImage++) {
					fprintf(file, "\nImage=%d\n", currImage);
					int numPn = errorProjPoints[currImage]->cols;
					for ( int currPoint = 0; currPoint < numPn; currPoint++ ) {
						double ex, ey;
						ex = cvmGet(errorProjPoints[currImage], 0, currPoint);
						ey = cvmGet(errorProjPoints[currImage], 1, currPoint);
						fprintf(file, "%lf,%lf\n", ex, ey);
					}
				}
				fprintf(file, "\n---- test 0 -----\n");
#endif

				fclose(file);
			}
#endif



			/* Compare new error and last error */
			if ( newError < prevError ) {
				/* accept new value */
				prevError = newError;
				/* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) )  */
				{
					double normAll1 = 0;
					double normAll2 = 0;
					double currNorm1 = 0;
					double currNorm2 = 0;
					/* compute norm for projection matrices */
					for ( currImage = 0; currImage < numImages; currImage++ ) {
						currNorm1 = cvNorm(newVectorX_projMatrs[currImage], vectorX_projMatrs[currImage]);
						currNorm2 = cvNorm(newVectorX_projMatrs[currImage]);

						normAll1 += currNorm1 * currNorm1;
						normAll2 += currNorm2 * currNorm2;
					}

					/* compute norm for points */
					currNorm1 = cvNorm(newVectorX_points4D, vectorX_points4D);
					currNorm2 = cvNorm(newVectorX_points4D);

					normAll1 += currNorm1 * currNorm1;
					normAll2 += currNorm2 * currNorm2;

					/* compute change */
					change = sqrt(normAll1) / sqrt(normAll2);


//#ifdef TRACK_BUNDLE
#if 1
					{
						/* Create file to track */
						FILE* file;
						file = fopen( TRACK_BUNDLE_FILE , "a");
						fprintf(file, "\nChange inside newVal change = %20.15lf\n", change);
						fprintf(file, "   normAll1= %20.15lf\n", sqrt(normAll1));
						fprintf(file, "   normAll2= %20.15lf\n", sqrt(normAll2));

						fclose(file);
					}
#endif

				}

				alpha /= 10;
				for ( currImage = 0; currImage < numImages; currImage++ ) {
					cvCopy(newVectorX_projMatrs[currImage], vectorX_projMatrs[currImage]);
				}
				cvCopy(newVectorX_points4D, vectorX_points4D);

				break;
			} else {
				alpha *= 10;
			}

		} while ( change > epsilon && currIter < maxIter ); /* solve normal equation using current alpha */

//#ifdef TRACK_BUNDLE
#if 1
		{
			FILE* file;
			file = fopen( TRACK_BUNDLE_FILE , "a");
			fprintf(file, "\nBest error = %40.35lf\n", prevError);
			fclose(file);
		}

#endif


	} while ( change > epsilon && currIter < maxIter );

	/*--------------------------------------------*/
	/* Optimization complete copy computed params */
	/* Copy projection matrices */
	for ( currImage = 0; currImage < numImages; currImage++ ) {
		cvCopy(newVectorX_projMatrs[currImage], resultProjMatrs[currImage]);
	}
	/* Copy 4D points */
	cvCopy(newVectorX_points4D, resultPoints4D);

//    free(memory);

	__END__;

	/* Free allocated memory */

	/* Free simple matrices */
	cvFree(&vectorX_points4D);
	cvFree(&newVectorX_points4D);
	cvFree(&changeVectorX_points4D);
	cvFree(&changeVectorX_projMatrs);
	cvFree(&matrW);
	cvFree(&workMatrVi);
	cvFree(&jacProjErr);
	cvFree(&jacPointErr);
	cvFree(&matrTmpSys1);
	cvFree(&matrSysDeltaP);
	cvFree(&vectTmpSys3);
	cvFree(&vectSysDeltaP);
	cvFree(&deltaP);
	cvFree(&deltaM);
	cvFree(&vectTmpSysM);

	/* Free arrays of matrices */
	icvFreeMatrixArray(&vectorX_projMatrs, numImages);
	icvFreeMatrixArray(&newVectorX_projMatrs, numImages);
	icvFreeMatrixArray(&observVisPoints, numImages);
	icvFreeMatrixArray(&projVisPoints, numImages);
	icvFreeMatrixArray(&errorProjPoints, numImages);
	icvFreeMatrixArray(&DerivProj, numImages);
	icvFreeMatrixArray(&DerivPoint, numImages);
	icvFreeMatrixArray(&matrsUk, numImages);
	icvFreeMatrixArray(&workMatrsUk, numImages);
	icvFreeMatrixArray(&matrsVi, numPoints);
	icvFreeMatrixArray(&workMatrsInvVi, numPoints);

	return;
}
